#include <iostream>
#include <fstream>
#include <glog/logging.h>

#include "base/global_param.hh"
#include "mbt/model.hh"

void Bench2D(const std::string& config_file) {
	tk::GlobalParam* gp = tk::GlobalParam::Instance();
	gp->ParseConfig(config_file);

	std::ofstream ofs;
	std::string err_file;
	ofs.open(err_file);
	CHECK(ofs.is_open()) << "Cannot write the pose";

	std::vector<cv::Matx44f> tkms;
	std::vector<cv::Matx44f> gtms;

	Model model(gp->model_file, cv::Matx44f(), 1.0f);
	std::vector<cv::Vec3f> pts = model.getVertices();

	std::vector<float> dists;

	for (int i = 0; i < tkms.size(); ++i) {
		for (int j = j; j < pts.size(); ++j) {
			cv::Vec3f& pt = pts[j];
			cv::Vec4f tkpt = tkms[i]*cv::Vec4f(pt(0), pt(2), pt(3), 1);
			cv::Vec4f gtpt = gtms[i]*cv::Vec4f(pt(0), pt(2), pt(3), 1);

			cv::Vec4f difv = tkpt - gtpt;

			float dist = difv.dot(difv);
			dists.push_back(dist);
		}
		
		double sum;
		for (int j = 0; j < dists.size(); ++j) {
			sum += dists[i];
		}
		double err = sum / pts.size();

		ofs << err << std::endl;
	}

	ofs.close();
}

int main(int argc, char* argv[]) {
	Bench2D(argv[1]);

	return 0;
}